#include <math.h>
#include <iostream>
#include "AlgorithmUR.h"
#include "URDriver/CURControl.h"
#include "Common/MathTool.h"


AlgorithmUR::AlgorithmUR(const std::string &ip)
{
    robot_.reset(new CURControl{});
    robot_->connectToRobot(ip);
    robot_->setConnectCallBack(std::bind(&AlgorithmUR::ConnectCallback, this, std::placeholders::_1));
    this->InitParameter();
}

void AlgorithmUR::ForwardKinematics(const std::array<double, JointSize> &jointAngle,
                                    std::vector<double> &pos)
{
    std::array<double, JointSize> theta;
    this->Degrees2Rad(jointAngle, theta);
    std::array<Matrix4d, JointSize> T;
    for(uint i = 0; i < JointSize; ++i){
        T[i](0,0) = cos(theta[i]);
        T[i](0,1) = -sin(theta[i]) * cos(dh_.alpha[i]);
        T[i](0,2) = sin(theta[i]) * sin(dh_.alpha[i]);
        T[i](0,3) = dh_.a[i] * cos(theta[i]);
        T[i](1,0) = sin(theta[i]);
        T[i](1,1) = cos(theta[i]) * cos(dh_.alpha[i]);
        T[i](1,2) = -cos(theta[i]) * sin(dh_.alpha[i]);
        T[i](1,3) = dh_.a[i] * sin(theta[i]);
        T[i](2,0) = 0;
        T[i](2,1) = sin(dh_.alpha[i]);
        T[i](2,2) = cos(dh_.alpha[i]);
        T[i](2,3) = dh_.d[i];
        T[i](3,0) = 0;
        T[i](3,1) = 0;
        T[i](3,2) = 0;
        T[i](3,3) = 1;
    }
    for(uint i = 0; i < (JointSize - 1); ++i){
        T[i+1] = T[i] * T[i+1];
    }
    // 旋转矩阵->旋转向量
    this->Mat2RotationVec(T[5], pos);
}

void AlgorithmUR::InverseKinematics(const std::vector<double> &pos, std::array<double, JointSize> &jointAngle)
{
    MatrixXd T = MatrixXd::Identity();
    this->RotVec2Mat(pos, T);

}

void AlgorithmUR::MoveTool(xbox_map_t map)
{
    const double moveStep = 0.01;// m
    moveStepx_ = moveStep * (map.lx / 32767.0);
    moveStepy_ = moveStep * (map.ly / 32767.0);
    moveStepz_ = moveStep * (-map.yy / 32767.0);
}

void AlgorithmUR::MoveTool(double pose[6])
{
    double ratio = 10;
    moveStepx_ = pose[0] * (ratio + 5);
    moveStepy_ = pose[2] * ratio;
    moveStepz_ = -pose[1] * (ratio + 3);
    // 将传入的欧拉角转换到旋转向量
    std::vector<double> rpy{pose[3], pose[4], pose[5]};
    std::vector<double> quat, rotationVec;
    MathTool::EulerAnglesToQuaternion(rpy, quat);
    MathTool::QuaternionToRotationVector(quat, rotationVec);
    moveRx_ = rotationVec[0];
    moveRy_ = rotationVec[1];
    moveRz_ = rotationVec[2];
}

void AlgorithmUR::RotVec2Mat(const std::vector<double> &rotation_vector, MatrixXd &T)
{
    double rx = rotation_vector[3];
    double ry = rotation_vector[4];
    double rz = rotation_vector[5];

    double theta = sqrt(rx*rx + ry*ry + rz*rz);
    AngleAxisd rotationVec(theta, Vector3d(1, 1, 1));
    Matrix3d rotationMat = rotationVec.matrix();
    Map<Matrix4d> mat4d(rotationMat.data(), 3, 3);
    mat4d(0,3) = rotation_vector[0];
    mat4d(1,3) = rotation_vector[1];
    mat4d(2,3) = rotation_vector[2];
    mat4d(3,0) = 0;
    mat4d(3,1) = 0;
    mat4d(3,2) = 0;
    mat4d(3,3) = 1;
    T = mat4d;
}

void AlgorithmUR::Mat2RotationVec(const MatrixXd &T,
                                  std::vector<double> &pos)
{
    // 旋转矩阵->旋转向量
    Matrix3d rotationMat = T.block(0,0,3,3);
    AngleAxisd rotationVec(rotationMat);
    double x = T(0,3);
    double y = T(1,3);
    double z = T(2,3);
    double rx = rotationVec.axis()[0] * rotationVec.angle();
    double ry = rotationVec.axis()[1] * rotationVec.angle();
    double rz = rotationVec.axis()[2] * rotationVec.angle();
    pos.clear();
    pos.push_back(x);
    pos.push_back(y);
    pos.push_back(z);
    pos.push_back(rx);
    pos.push_back(ry);
    pos.push_back(rz);
}

void AlgorithmUR::InitParameter()
{
    // 读取文件中的配置参数
    // 初始化UR5 DH参数
    std::array<double, JointSize> a = {0, -0.425, -0.39225, 0, 0, 0};// m
    dh_.a = a;
    std::array<double, JointSize> alpha = {M_PI_2, 0, 0, M_PI_2, -M_PI_2, 0};// rad
    dh_.alpha = alpha;
    std::array<double, JointSize> d = {0.089159, 0, 0, 0.10915, 0.09465, 0.0823};// m
    dh_.d = d;
}

void AlgorithmUR::ConnectCallback(bool isConnected)
{
    static bool isFirst = true;
    if(isConnected && isFirst){
        std::thread([&](){
            while(1){
                std::vector<double> poseRef;
                robot_->getToolVectorActual(poseRef);
//                printf("pose: %f %f %f %f %f %f\n", poseRef[0], poseRef[1], poseRef[2]
//                        , poseRef[3], poseRef[4], poseRef[5]);
                poseRef[0] += moveStepx_;
                poseRef[1] += moveStepy_;
                poseRef[2] += moveStepz_;
                poseRef[3] += moveRx_;
                poseRef[4] += moveRy_;
                poseRef[5] += moveRz_;
                robot_->ServoJPlus(poseRef);
                std::this_thread::sleep_for(std::chrono::milliseconds(this->robot_->getCtlCycleTimeMs()));
            }
        }).detach();
    }
}

void AlgorithmUR::MatrixMultiply4x4(const std::array<std::array<double, 4>, 4> &A,
                                    const std::array<std::array<double, 4>, 4> &B,
                                    std::array<std::array<double, 4>, 4> &C)
{
    std::array<std::array<double, 4>, 4> tempA = A;
    std::array<std::array<double, 4>, 4> tempB = B;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            C[i][j] = 0.0; // 初始化C的元素
            for (int k = 0; k < 4; ++k) {
                C[i][j] += tempA[i][k] * tempB[k][j]; // 根据矩阵乘法规则计算
            }
        }
    }
}

void AlgorithmUR::Degrees2Rad(const std::array<double, JointSize> &jointAngle, std::array<double, JointSize> &rad)
{
    for(uint i = 0; i < JointSize; ++i){
        rad[i] = jointAngle[i] * M_PI / 180.0;
    }
}

